

#include <stdbool.h>
#include <stdint.h>
#include <string.h>

#include "platform.h"

#include "drivers/adc.h"
#include "drivers/light_led.h"
#include "drivers/system.h"
#include "drivers/time.h"
#include "common/time.h"
#include "common/streambuf.h"
#include "common/utils.h"
#include "build/debug.h"

#include "flight/altitude.h"
#include "io/serial.h"

#include "msp/msp.h"
#include "msp/msp_serial.h"
#include "flight/imu.h"

static mspPort_t _mspPort;
#ifdef SEND_ATTITUDE_OVER_SERIAL
static mspPort_t _serialPort;
#endif

void mspSerialInit(void)
{
    memset(&_mspPort, 0, sizeof(_mspPort));
    mspSerialAllocatePorts();
}


static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort)
{
    memset(mspPortToReset, 0, sizeof(mspPort_t));
    mspPortToReset->port = serialPort;
}

void mspSerialAllocatePorts()
{
    serialPortConfig_t *portConfig = findSerialPortConfig(0);

    serialPort_t *serialPort = openSerialPort(portConfig->identifier, FUNCTION_MSP, NULL, baudRates[portConfig->baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
    if (serialPort) {
        resetMspPort(&_mspPort, serialPort);
    }
#ifdef SEND_ATTITUDE_OVER_SERIAL
    serialPortConfig_t *portConfig1 = findSerialPortConfig(1);
    serialPort_t *serialPort1 = openSerialPort(portConfig1->identifier, FUNCTION_MSP, NULL, baudRates[portConfig1->baudrateIndex], MODE_RXTX, SERIAL_NOT_INVERTED);
    if (serialPort1) {
        resetMspPort(&_serialPort, serialPort1);
    }
#endif
}

static bool mspSerialProcessReceivedData(mspPort_t *mspPort, uint8_t c)
{
    if (mspPort->c_state == MSP_IDLE) {
        if (c == '$') {
            mspPort->c_state = MSP_HEADER_START;
        } else {
            return false;
        }
    } else if (mspPort->c_state == MSP_HEADER_START) {
        mspPort->c_state = (c == 'M') ? MSP_HEADER_M : MSP_IDLE;
    } else if (mspPort->c_state == MSP_HEADER_M) {
        mspPort->c_state = MSP_IDLE;
        switch (c) {
            case '<': // COMMAND
                mspPort->packetType = MSP_PACKET_COMMAND;
                mspPort->c_state = MSP_HEADER_ARROW;
                break;
            case '>': // REPLY
                mspPort->packetType = MSP_PACKET_REPLY;
                mspPort->c_state = MSP_HEADER_ARROW;
                break;
            default:
                break;
        }
    } else if (mspPort->c_state == MSP_HEADER_ARROW) {
        if (c > MSP_PORT_INBUF_SIZE) {
            mspPort->c_state = MSP_IDLE;
        } else {
            mspPort->dataSize = c;
            mspPort->offset = 0;
            mspPort->checksum = 0;
            mspPort->checksum ^= c;
            mspPort->c_state = MSP_HEADER_SIZE;
        }
    } else if (mspPort->c_state == MSP_HEADER_SIZE) {
        mspPort->cmdMSP = c;
        mspPort->checksum ^= c;
        mspPort->c_state = MSP_HEADER_CMD;
    } else if (mspPort->c_state == MSP_HEADER_CMD && mspPort->offset < mspPort->dataSize) {
        mspPort->checksum ^= c;
        mspPort->inBuf[mspPort->offset++] = c;
    } else if (mspPort->c_state == MSP_HEADER_CMD && mspPort->offset >= mspPort->dataSize) {
        if (mspPort->checksum == c) {
            mspPort->c_state = MSP_COMMAND_RECEIVED;
        } else {
            mspPort->c_state = MSP_IDLE;
        }
    }
    return true;
}

static uint8_t mspSerialChecksumBuf(uint8_t checksum, const uint8_t *data, int len)
{
    while (len-- > 0) {
        checksum ^= *data++;
    }
    return checksum;
}

#define JUMBO_FRAME_SIZE_LIMIT 255

static int mspSerialEncode(mspPort_t *msp, mspPacket_t *packet)
{
    serialBeginWrite(msp->port);
    const int len = sbufBytesRemaining(&packet->buf);
    const int mspLen = len < JUMBO_FRAME_SIZE_LIMIT ? len : JUMBO_FRAME_SIZE_LIMIT;
    uint8_t hdr[8] = {
        '$',
        'M',
        packet->result == MSP_RESULT_ERROR ? '!' : packet->direction == MSP_DIRECTION_REPLY ? '>' : '<',
        mspLen,
        packet->cmd
    };
    int hdrLen = 5;
#define CHECKSUM_STARTPOS 3  // checksum starts from mspLen field
    if (len >= JUMBO_FRAME_SIZE_LIMIT) {
        hdrLen += 2;
        hdr[5] = len & 0xff;
        hdr[6] = (len >> 8) & 0xff;
    }
    serialWriteBuf(msp->port, hdr, hdrLen);
    uint8_t checksum = mspSerialChecksumBuf(0, hdr + CHECKSUM_STARTPOS, hdrLen - CHECKSUM_STARTPOS);
    if (len > 0) {
        serialWriteBuf(msp->port, sbufPtr(&packet->buf), len);
        checksum = mspSerialChecksumBuf(checksum, sbufPtr(&packet->buf), len);
    }
    serialWriteBuf(msp->port, &checksum, 1);
    serialEndWrite(msp->port);
    //serialWrite(msp->port, checksum);
    return sizeof(hdr) + len + 1; // header, data, and checksum
}

static mspPostProcessFnPtr mspSerialProcessReceivedCommand(mspPort_t *msp, mspProcessCommandFnPtr mspProcessCommandFn)
{
    static uint8_t outBuf[MSP_PORT_OUTBUF_SIZE];

    mspPacket_t reply = {
        .buf = { .ptr = outBuf, .end = ARRAYEND(outBuf), },
        .cmd = -1,
        .result = 0,
        .direction = MSP_DIRECTION_REPLY,
    };
    uint8_t *outBufHead = reply.buf.ptr;

    mspPacket_t command = {
        .buf = { .ptr = msp->inBuf, .end = msp->inBuf + msp->dataSize, },
        .cmd = msp->cmdMSP,
        .result = 0,
        .direction = MSP_DIRECTION_REQUEST,
    };

    mspPostProcessFnPtr mspPostProcessFn = NULL;
    const mspResult_e status = mspProcessCommandFn(&command, &reply, &mspPostProcessFn);

    if (status != MSP_RESULT_NO_REPLY) {
        sbufSwitchToReader(&reply.buf, outBufHead); // change streambuf direction
        mspSerialEncode(msp, &reply);
    }

    return mspPostProcessFn;
}


static void mspSerialProcessReceivedReply(mspPort_t *msp, mspProcessReplyFnPtr mspProcessReplyFn)
{
    mspPacket_t reply = {
        .buf = {
            .ptr = msp->inBuf,
            .end = msp->inBuf + msp->dataSize,
        },
        .cmd = msp->cmdMSP,
        .result = 0,
    };

    mspProcessReplyFn(&reply);

    msp->c_state = MSP_IDLE;
}

// #ifdef PERFORM_TEST
// volatile int temp;

// volatile int data0[64];
// volatile int data1[64];
// volatile int data2[64];
// #endif

timeMs_t lastTime;

void mspSerialProcess(mspEvaluateNonMspData_e evaluateNonMspData, mspProcessCommandFnPtr mspProcessCommandFn, mspProcessReplyFnPtr mspProcessReplyFn)
{
#ifdef SEND_ATTITUDE_OVER_SERIAL
    mspPort_t * const serialPort = &_serialPort;
    timeMs_t time = millis();
    if(time - lastTime > 1000){
        int32_t att_roll = (int32_t)(attitude.values.att_roll * 10000);// 右+ 左-
        int32_t att_pitch = (int32_t)(attitude.values.att_pitch * 10000);
        lastTime = time;
        serialWrite(serialPort->port, 'a');
        serialWrite(serialPort->port, att_roll & 0xFF);
        serialWrite(serialPort->port, (att_roll >>  8) & 0xFF);
        serialWrite(serialPort->port, (att_roll >> 16) & 0xFF);
        serialWrite(serialPort->port, att_pitch & 0xFF);
        serialWrite(serialPort->port, (att_pitch >>  8) & 0xFF);
        serialWrite(serialPort->port, (att_pitch >> 16) & 0xFF);
        att_roll++;
        att_pitch++;
        serialWrite(serialPort->port, att_roll & 0xFF);
        serialWrite(serialPort->port, (att_roll >>  8) & 0xFF);
        serialWrite(serialPort->port, (att_roll >> 16) & 0xFF);
        serialWrite(serialPort->port, att_pitch & 0xFF);
        serialWrite(serialPort->port, (att_pitch >>  8) & 0xFF);
        serialWrite(serialPort->port, (att_pitch >> 16) & 0xFF);
    }
#endif
// #ifdef PERFORM_TEST
//     timeUs_t t0;
//     timeUs_t t1;
    
//     temp = 1;
    
//     for(int i=0;i<64;i++){
//         data0[i] = temp;
//         data1[i] = temp;
//     }
// //int    v  and10:10707,and:1071,add10: 9827,add: 983,sub10:10705,sub:1071,mul10: 9827,mul:1071,div10:13388,div:1426,
// //int   nv  and10: 8956,and: 896,add10: 9827,add: 984,sub10: 8953,sub: 896,mul10: 8953,mul: 984,div10:13388,div:1252,
// //float  v  and10:    0,and:   0,add10:35628,add:3563,sub10:30274,sub:3027,mul10:32952,mul:3301,div10:28505,div:2851,
// //float nv  and10:    0,and:   0,add10:37406,add:3652,sub10:30288,sub:3029,mul10:33840,mul:3385,div10:30303,div:2763,

// //SLOW_128k
// //int    v  and10:13385,and:1344,add10:16013,add:1335,sub10:13427,sub:1340,mul10:13344,mul:1335,div10:17812,div:1958,
// //int   nv  and10:13345,and:1337,add10:13344,add:1078,sub10:13359,sub:1336,mul10:10771,mul:1337,div10:17807,div:1870,
// //float  v  and10:    0,and:   0,add10:61504,add:6143,sub10:50828,sub:5075,mul10:56167,mul:5608,div10:50829,div:4821,
// //float nv  and10:    0,and:   0,add10:61503,add:6151,sub10:50829,sub:5084,mul10:56165,mul:5618,div10:50828,div:5072,

//   {
//    t0 = micros();
//    for(int j=0;j<1000;j++){
//        for(int i=0;i<64;i++){
//            data2[i] = data0[i] & data1[i];
//        }
//    }
//    t1 = micros(); debuginfoFastInt[0] = (t1-t0);
//   }
  
//   {
//    t0 = micros();
//    for(int j=0;j<100;j++){
//        for(int i=0;i<64;i++){
//            data2[i] = data0[i] & data1[i];
//        }
//    }
//    t1 = micros(); debuginfoFastInt[1] = (t1-t0);
//   }
  
//   {
//     t0 = micros();
//     for(int j=0;j<1000;j++){
//         for(int i=0;i<64;i++){
//             data2[i] = data0[i] + data1[i];
//         }
//     }
//     t1 = micros(); debuginfoFastInt[2] = (t1-t0);
//   }
  
//   {
//     t0 = micros();
//     for(int j=0;j<100;j++){
//         for(int i=0;i<64;i++){
//             data2[i] = data0[i] + data1[i];
//         }
//     }
//     t1 = micros(); debuginfoFastInt[3] = (t1-t0);
//   }
  
//   {
//     t0 = micros();
//     for(int j=0;j<1000;j++){
//         for(int i=0;i<64;i++){
//             data2[i] = data0[i] - data1[i];
//         }
//     }
//     t1 = micros(); debuginfoFastInt[4] = (t1-t0);
//   }
  
//   {
//     t0 = micros();
//     for(int j=0;j<100;j++){
//         for(int i=0;i<64;i++){
//             data2[i] = data0[i] - data1[i];
//         }
//     }
//     t1 = micros(); debuginfoFastInt[5] = (t1-t0);
//   }
  
//   {
//     t0 = micros();
//     for(int j=0;j<1000;j++){
//         for(int i=0;i<64;i++){
//             data2[i] = data0[i] * data1[i];
//         }
//     }
//     t1 = micros(); debuginfoFastInt[6] = (t1-t0);
//   }
  
//   {
//     t0 = micros();
//     for(int j=0;j<100;j++){
//         for(int i=0;i<64;i++){
//             data2[i] = data0[i] * data1[i];
//         }
//     }
//     t1 = micros(); debuginfoFastInt[7] = (t1-t0);
//   }
  
  
//   {
//     t0 = micros();
//     for(int j=0;j<1000;j++){
//         for(int i=0;i<64;i++){
//             data2[i] = data0[i] / data1[i];
//         }
//     }
//     t1 = micros(); debuginfoFastInt[8] = (t1-t0);
//   }
  
//   {
//     t0 = micros();
//     for(int j=0;j<100;j++){
//         for(int i=0;i<64;i++){
//             data2[i] = data0[i] / data1[i];
//         }
//     }
//     t1 = micros(); debuginfoFastInt[9] = (t1-t0);
//   }

//     for(int i=0;i<64;i++){
//         temp = data2[i];
//     }
// #endif


    mspPort_t * const mspPort = &_mspPort;

    mspPostProcessFnPtr mspPostProcessFn = NULL;

    while (serialRxBytesWaiting(mspPort->port)) {

        const uint8_t c = serialRead(mspPort->port);
        const bool consumed = mspSerialProcessReceivedData(mspPort, c);

        if (!consumed && evaluateNonMspData == MSP_EVALUATE_NON_MSP_DATA) {
        }

        if (mspPort->c_state == MSP_COMMAND_RECEIVED) {
            if (mspPort->packetType == MSP_PACKET_COMMAND) {
                mspPostProcessFn = mspSerialProcessReceivedCommand(mspPort, mspProcessCommandFn);
            } else if (mspPort->packetType == MSP_PACKET_REPLY) {
                mspSerialProcessReceivedReply(mspPort, mspProcessReplyFn);
            }

            mspPort->c_state = MSP_IDLE;
            break; // process one command at a time so as not to block.
        }
    }

    if (mspPostProcessFn) {
        waitForSerialPortToFinishTransmitting(mspPort->port);
        mspPostProcessFn(mspPort->port);
    }
}
